from global_var import *
import global_var
from All_Init import *
import All_Init
dt = 0.005
last_angle = 0
first_angle = 0
global_var.gyro_ratio = 5.02
global_var.acc_ratio = 1.6
def angle_calc(angle_m, gyro_m):
    temp_angle = 0
    gyro_now = 0
    error_angle = 0
    global last_angle
    global first_angle
 
    if first_angle != 0:
        first_angle = 1
        last_angle = angle_m
    
    gyro_now = gyro_m * global_var.gyro_ratio
    error_angle = (angle_m - last_angle) * global_var.acc_ratio
    temp_angle = last_angle + (error_angle + gyro_now)*dt
    last_angle = temp_angle
    return temp_angle
# -------编码器滤波----- #
last_speed = 0
real_speed = 0
right_speed = 0
left_speed = 0
def Encoder_filter():
    global last_speed,real_speed
    global left_speed,right_speed
    global_var.Speed_L = All_Init.encoder_l.get()
    global_var.Speed_R = All_Init.encoder_r.get()
    last_speed = (global_var.Speed_L+global_var.Speed_R)//2
    real_speed *=0.75
    real_speed += last_speed*0.25
    return real_speed